#include "HiranipraShared.h"

#ifndef _SHARED_MATH_VECTOR_H_
#define _SHARED_MATH_VECTOR_H_

namespace Hiranipra {
	namespace Math {

		//////////////////////////////////////////////////////////////////////////
		// Class: Vector
		//========================================================================
		// Code for this class taken without permission *grin* from http://www.baskuenen.myweb.nl/
		// Optimized by ben.vanik@gmail.com
		class Vector
		{
		public:
			float	x, y, z;

			Vector()
			{
			}

			Vector( float _x, float _y, float _z )
			{
#if defined( X86_MATH )
				__asm
				{
					mov ebx, dword ptr [this]
					mov eax, _x
					mov dword ptr [ebx], eax
					mov eax, _y
					mov dword ptr [ebx+4], eax
					mov eax, _z
					mov dword ptr [ebx+8], eax
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x	= _x;
				y	= _y;
				z	= _z;
#endif
			}

			Vector( const Vector &_v )
			{
#if defined( X86_MATH )
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]

					mov eax, dword ptr [ecx]
					mov dword ptr [ebx], eax
					mov eax, dword ptr [ecx+4]
					mov dword ptr [ebx+4], eax
					mov eax, dword ptr [ecx+8]
					mov dword ptr [ebx+8], eax
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x	= _v.x;
				y	= _v.y;
				z	= _v.z;
#endif
			}

			const Vector& operator = ( const Vector& _v )
			{
#if defined( X86_MATH )
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]
					
					mov eax, dword ptr [ecx]
					mov dword ptr [ebx], eax
					mov eax, dword ptr [ecx+4]
					mov dword ptr [ebx+4], eax
					mov eax, dword ptr [ecx+8]
					mov dword ptr [ebx+8], eax
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x	= _v.x;
				y	= _v.y;
				z	= _v.z;
#endif
				return(*this);
			}

			// Boolean stuff
			const bool operator == ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				bool	bEqual;
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]
					fld [ebx]				; x
					fcomp [ecx]				; _v.x
					fnstsw ax   
					test ah, 44h
					jp end_no
					fld [ebx+4]				; y
					fcomp [ecx+4]			; _v.y
					fnstsw ax   
					test ah, 44h
					jp end_no
					fld [ebx+8]				; z
					fcomp [ecx+8]			; _v.z
					fnstsw ax   
					test ah, 44h
					jp end_no
					mov bEqual, 1
					jmp end
				end_no:
					mov bEqual, 0
				end:
				}
				return( bEqual );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return( (x == _v.x) && (y == _v.y) && (z == _v.z) );
#endif
			}

			const bool operator != ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				bool	bEqual;
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]
					fld [ebx]				; x
					fcomp [ecx]				; _v.x
					fnstsw ax   
					test ah, 44h
					jp end_no
					fld [ebx+4]				; y
					fcomp [ecx+4]			; _v.y
					fnstsw ax   
					test ah, 44h
					jp end_no
					fld [ebx+8]				; z
					fcomp [ecx+8]			; _v.z
					fnstsw ax   
					test ah, 44h
					jp end_no
					mov bEqual, 1
					jmp end
				end_no:
					mov bEqual, 0
				end:
				}
				return( !bEqual );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return( !((*this) == _v) );
#endif
			}

			// Add & Substract, Positive & Negative
			const Vector operator + ( const Vector& _v ) const
			{
				Vector vta;
#if defined( X86_MATH )
				__asm
				{
					; save needed pointers
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]

					; x
					fld dword ptr [ecx]		; load x of _v
					fadd dword ptr [edx]	; load x of this
					fst dword ptr [vta]		; store to x of vta

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fadd dword ptr [edx+4]	; load y of this
					fst dword ptr [vta+4]	; store to y of vta
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fadd dword ptr [edx+8]	; load z of this
					fst dword ptr [vta+8]	; store to z of vta
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				vta.x	= x + _v.x;
				vta.y	= y + _v.y;
				vta.z	= z + _v.z;
#endif
				return( vta );
			}

			const Vector operator + () const
			{
				Vector vta;
#if defined( X86_MATH )
				__asm
				{
					mov ebx, dword ptr [this]
					;x
					mov ecx, dword ptr [ebx]
					mov dword ptr [vta], ecx
					;y
					mov ecx, dword ptr [ebx+4]
					mov dword ptr [vta+4], ecx
					;z
					mov ecx, dword ptr [ebx+8]
					mov dword ptr [vta+8], ecx
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				vta.x	= x;
				vta.y	= y;
				vta.z	= z;
#endif
				return( vta );
			}

			const Vector& operator += ( const Vector& _v )
			{
#if defined( X86_MATH )
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]
					;x
					fld [ebx]
					fadd [ecx]
					fst [ebx]
					;y
					fld [ebx+4]
					fadd [ecx+4]
					fst [ebx+4]
					;z
					fld [ebx+8]
					fadd [ecx+8]
					fst [ebx+8]
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x += _v.x;
				y += _v.y;
				z += _v.z;
#endif
				return( *this );
			}

			const Vector operator - ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]
					
					; x
					fld dword ptr [ecx]		; load x of _v
					fsub dword ptr [edx]	; load x of this
					fst dword ptr [vta]		; store to x of vta

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fsub dword ptr [edx+4]	; load y of this
					fst dword ptr [vta+4]	; store to y of vta
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fsub dword ptr [edx+8]	; load z of this
					fst dword ptr [vta+8]	; store to z of vta

				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return( Vector(x - _v.x, y - _v.y, z - _v.z) );
#endif
			}

			const Vector operator - () const
			{
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov ebx, dword ptr [this]
					;x
					fld [ebx]
					fchs
					fst dword ptr [vta]
					;y
					fld [ebx+4]
					fchs
					fst dword ptr [vta+4]
					;z
					fld [ebx+8]
					fchs
					fst dword ptr [vta+8]
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return( Vector(-x, -y, -z) );
#endif
			}

			const Vector& operator -= ( const Vector& _v )
			{
#if defined( X86_MATH )
				/*
				x -= y
				x = x - y
				*/
				__asm
				{
					mov ebx, dword ptr [this]
					mov ecx, dword ptr [_v]
					;x
					fld [ecx]
					fld [ebx]
					fsub
					fst [ebx]
					;y
					fld [ecx+4]
					fld [ebx+4]
					fsub
					fst [ebx+4]
					;z
					fld [ecx+8]
					fld [ebx+8]
					fsub
					fst [ebx+8]
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x -= _v.x;
				y -= _v.y;
				z -= _v.z;
#endif
				return( (*this) );
			}

			// Multiply & Divide
			const Vector operator * ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				Vector	vta;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]
					
					; x
					fld dword ptr [ecx]		; load x of _v
					fmul dword ptr [edx]	; load x of this
					fst dword ptr [vta]		; store to x of vta

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fmul dword ptr [edx+4]	; load y of this
					fst dword ptr [vta+4]	; store to y of vta
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fmul dword ptr [edx+8]	; load z of this
					fst dword ptr [vta+8]	; store to z of vta
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return Vector(x * _v.x, y * _v.y, z * _v.z);
#endif
			}

			const Vector& operator *= ( const Vector& _v )
			{
#if defined( X86_MATH )
				Vector	vta;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]
					
					; x
					fld dword ptr [ecx]		; load x of _v
					fmul dword ptr [edx]	; load x of this
					fst dword ptr [edx]		; store to x of this

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fmul dword ptr [edx+4]	; load y of this
					fst dword ptr [edx+4]	; store to y of this
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fmul dword ptr [edx+8]	; load z of this
					fst dword ptr [edx+8]	; store to z of this
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x *= _v.x;
				y *= _v.y;
				z *= _v.z;
#endif
                return( (*this) );
			}

			const Vector operator * ( const float d ) const
			{
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov edx, dword ptr [this]
					fld d					; load d
					fld st					; dup it
					fld st					; dup it

					; x
					fmul dword ptr [edx]	; load x of this
					fstp dword ptr [vta]	; store to x of vta

					; y
					fmul dword ptr [edx+4]	; load y of this
					fstp dword ptr [vta+4]	; store to y of vta
					
					; z
					fmul dword ptr [edx+8]	; load z of this
					fstp dword ptr [vta+8]	; store to z of vta
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return Vector(x * d, y * d, z * d);
#endif
			}

			const Vector& operator *= ( const float d )
			{
#if defined( X86_MATH )
				__asm
				{
					mov edx, dword ptr [this]
					fld d					; load d
					fld st
					fld st
					
					; x
					fmul dword ptr [edx]	; load x of this
					fstp dword ptr [edx]	; store to x of this

					; y
					fmul dword ptr [edx+4]	; load y of this
					fstp dword ptr [edx+4]	; store to y of this
					
					; z
					fmul dword ptr [edx+8]	; load z of this
					fstp dword ptr [edx+8]	; store to z of this
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x *= d;
				y *= d;
				z *= d;
#endif
				return( (*this) );
			}

			const Vector operator / ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]
					
					; x
					fld dword ptr [ecx]		; load x of _v
					fdiv dword ptr [edx]	; load x of this
					fstp dword ptr [vta]	; store to x of vta

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fdiv dword ptr [edx+4]	; load y of this
					fstp dword ptr [vta+4]	; store to y of vta
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fdiv dword ptr [edx+8]	; load z of this
					fstp dword ptr [vta+8]	; store to z of vta
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return Vector(x / _v.x, y / _v.y, z / _v.z);
#endif
			}

			const Vector& operator /= ( const Vector& _v )
			{
#if defined( X86_MATH )
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]
					
					; x
					fld dword ptr [ecx]		; load x of _v
					fdiv dword ptr [edx]	; load x of this
					fstp dword ptr [edx]	; store to x of this

					; y
					fld dword ptr [ecx+4]	; load y of _v
					fdiv dword ptr [edx+4]	; load y of this
					fstp dword ptr [edx+4]	; store to y of this
					
					; z
					fld dword ptr [ecx+8]	; load z of _v
					fdiv dword ptr [edx+8]	; load z of this
					fstp dword ptr [edx+8]	; store to z of this
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x /= _v.x;
				y /= _v.y;
				z /= _v.z;
#endif
				return( (*this) );
			}

			const Vector operator / ( const float d ) const
			{
				const float _d = 1.0f / d;
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov edx, dword ptr [this]
					fld _d					; load _d
					fld st
					fld st

					; x
					fmul dword ptr [edx]	; load x of this
					fstp dword ptr [vta]	; store to x of vta

					; y
					fmul dword ptr [edx+4]	; load y of this
					fstp dword ptr [vta+4]	; store to y of vta
					
					; z
					fmul dword ptr [edx+8]	; load z of this
					fstp dword ptr [vta+8]	; store to z of vta
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return Vector(x * _d, y * _d, z * _d);
#endif
			}

			const Vector& operator /= ( const float d )
			{
				const float _d = 1.0f / d;
#if defined( X86_MATH )
				__asm
				{
					mov edx, dword ptr [this]
					fld _d					; load _d
					fld st
					fld st
					
					; x
					fmul dword ptr [edx]	; load x of this
					fstp dword ptr [edx]	; store to x of this

					; y
					fmul dword ptr [edx+4]	; load y of this
					fstp dword ptr [edx+4]	; store to y of this
					
					; z
					fmul dword ptr [edx+8]	; load z of this
					fstp dword ptr [edx+8]	; store to z of this
				}
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				x *= _d;
				y *= _d;
				z *= _d;
#endif
				return( (*this) );
			}

			// Dot Product
			const float operator % ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				float fTemp;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]

					; (x * _v.x)
					fld dword ptr [edx]
					fld dword ptr [ecx]
					fmul
					
					; (y * _v.y)
					fld dword ptr [edx+4]
					fld dword ptr [ecx+4]
					fmul

					; (z * _v.z)
					fld dword ptr [edx+8]
					fld dword ptr [ecx+8]
					fmul

					; (a) + (b) + (c)
					fadd
					fadd
					fst fTemp
				}
				return( fTemp );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return( (x * _v.x) + (y * _v.y) + (z * _v.z) );
#endif
			}

			// Cross Product
			const Vector operator ^ ( const Vector& _v ) const
			{
#if defined( X86_MATH )
				Vector vta;
				__asm
				{
					mov ecx, dword ptr [_v]
					mov edx, dword ptr [this]

					;x
					;= (_v.y * z)
					fld dword ptr [ecx+4]
					fld dword ptr [edx+8]
					fmul
					;= (y * _v.z)
					fld dword ptr [edx+4]
					fld dword ptr [ecx+8]
					fmul
					;= (a) - (b)
					fsub
					fstp dword ptr [vta]

					;y
					;= (_v.z * x)
					fld dword ptr [ecx+8]
					fld dword ptr [edx]
					fmul
					;= (z * _v.x)
					fld dword ptr [edx+8]
					fld dword ptr [ecx]
					fmul
					;= (a) - (b)
					fsub
					fstp dword ptr [vta+4]

					;z
					;= (_v.x * y)
					fld dword ptr [ecx]
					fld dword ptr [edx+4]
					fmul
					;= (x * _v.y)
					fld dword ptr [edx]
					fld dword ptr [ecx+4]
					fmul
					;= (a) - (b)
					fsub
					fstp dword ptr [vta+8]
				}
				return( vta );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return Vector(
						y * _v.z - _v.y * z,
						z * _v.x - _v.z * x,
						x * _v.y - _v.x * y);
#endif
			}

			const Vector& operator ^= ( const Vector& _v )
			{
				//TODO: Optimize this - Vector::operator ^=
				return( (*this) = (*this) ^ _v );
			}

			// Set length (Normalize if 1)
			const float operator ! () const
			{
#if defined( X86_MATH )
				float fTemp;
				__asm
				{
					mov ebx, dword ptr [this]

					;x
					fld dword ptr [ebx]
					fld st
					fmul

					;y
					fld dword ptr [ebx+4]
					fld st
					fmul

					;z
					fld dword ptr [ebx+8]
					fld st
					fmul

					; add them up
					fadd
					fadd

					; square root
					fsqrt

					fstp fTemp
				}
				return( fTemp );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				return SQRT( x * x + y * y + z * z );
#endif
			}

			const Vector operator | ( const float length ) const
			{
				//TODO: Optimize this - Vector::operator |
				return( (*this) * (length / !(*this)) );
			}

			const Vector& operator |= ( const float length )
			{
				//TODO: Optimize this - Vector::operator |=
				return *this = *this | length;
			}

			// The angle between two vectors in radians
			const float inline Angle( const Vector& normal ) const
			{
				//TODO: Optimize this - Vector::Angle
				return( acosf( (*this) % normal ) );
			}

			// Reflect in Normal _v
			const Vector inline Reflection( const Vector& planeNormal ) const
			{
				//TODO: Optimize this - Vector::Reflection
				return( ((*this) - planeNormal * 2.0 * ((*this) % planeNormal)) * !(*this) );
			}

			// Rotate fAngle Degrees (radians) Around Normal
			const Vector inline Rotate( const float angle, const Vector& normal ) const
			{
#if defined( X86_MATH )
				float cos0 = cosf( angle );
				float sin0 = sinf( angle );
#elif defined( SSE_MATH )
#error SSE Vector not implemented
#else
				float cos0;
				__asm
				{
					fld angle
					fcos
					fstp cos0
				}
				float sin0;
				__asm
				{
					fld angle
					fsin
					fstp sin0
				}
#endif
				//TODO: Optimize this - Vector::Rotate
				return( Vector(
						(*this) * cos0 + ((normal * (*this)) * (1.0f - cos0)) * normal + ((*this) ^ normal) * sin0
						) );
			}

			Point3D ToPoint3D()
			{
				return Point3D( x, y, z );
			}
		};

		inline float Distance( const Vector& v1, const Vector& v2 )
		{
			return SQRT( ( v2.x - v1.x ) * ( v2.x - v1.x ) + ( v2.y - v1.y ) * ( v2.y - v1.y ) + ( v2.z - v1.z ) * ( v2.z - v1.z ) );
		}

		inline float Distance( const Point3D& v1, const Vector& v2 )
		{
			return SQRT( ( v2.x - v1.x ) * ( v2.x - v1.x ) + ( v2.y - v1.y ) * ( v2.y - v1.y ) + ( v2.z - v1.z ) * ( v2.z - v1.z ) );
		}

	}
}

#endif // _SHARED_MATH_VECTOR_H_
